COMMENT ⊗   VALID 00002 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	GP[1:5]:	The initial approximations for the parameters to
C00006 ENDMK
C⊗;
COMMENT GP[1:5]:	The initial approximations for the parameters to
	be adjusted.  These are, in order, azimuth from camera 1 to
	camera 2 (clockwise from above), elevation from camera 1 to
	camera 2 (up), pan of camera 2 (clockwise from above), tilt of
	camera 2 (up), and roll of camera 2 (clockwise as seen looking
	out from the camera), all relative to the orientation of camera 1.;
BEGIN "TSTMAT"
REQUIRE "MAT3D.REL[VIS,HPM]" LOAD_MODULE;
REAL ARRAY PTR,RTP[1:3,1:3], V,C[1:3];
STRING S,SV,SPTR; INTEGER N,FOO,I,J; REAL P,T,R,SP,CP,ST,CT,SR,CR,DET;

EXTERNAL PROCEDURE M33X33(REFERENCE REAL MA,MB,MR);
EXTERNAL PROCEDURE M13X33(REFERENCE REAL MA,MB,MR);
EXTERNAL PROCEDURE T33(REFERENCE REAL MA,MR);

SV←"1 2 4";
SPTR←"0 0 0";

WHILE TRUE DO
   BEGIN

   OUTSTR("VECTOR: ");  LODED(SV&'15&'12); SV←S←INCHWL;
   V[1]←REALSCAN(S,FOO);
   V[2]←REALSCAN(S,FOO);
   V[3]←REALSCAN(S,FOO);

   OUTSTR("PAN, TILT, ROLL: "); LODED(SPTR&'15&'12); SPTR←S←INCHWL;
   P←REALSCAN(S,FOO); T←REALSCAN(S,FOO); R←REALSCAN(S,FOO);
   SP←SIN(P); CP←COS(P);
   ST←SIN(T); CT←COS(T);
   SR←SIN(R); CR←COS(R);

   PTR[1,1]←SP*SR*ST + CP*CR; PTR[1,2]←CT*SR;  PTR[1,3]←  CR*SP - CP*SR*ST;

   PTR[2,1]←CR*SP*ST - CP*SR; PTR[2,2]←CR*CT;  PTR[2,3]← -CP*CR*ST - SP*SR;

   PTR[3,1]←   -CT*SP       ; PTR[3,2]← ST  ;  PTR[3,3]← CP*CT            ;


   T33(PTR[1,1],PTR[1,1]);

   SETFORMAT(7,3);
   OUTSTR('15&'12);

   DET←PTR[1,1]*(PTR[2,2]*PTR[3,3]-PTR[2,3]*PTR[3,2])
      -PTR[2,1]*(PTR[1,2]*PTR[3,3]-PTR[1,3]*PTR[3,2])
      +PTR[3,1]*(PTR[1,2]*PTR[2,3]-PTR[1,3]*PTR[2,2]);
   

   FOR I←1,2,3 DO
      BEGIN
      OUTSTR("|");
      FOR J←1,2,3 DO OUTSTR(CVF(PTR[I,J]));
      OUTSTR("|");
      IF I=2 THEN OUTSTR(" = "&CVF(DET));
      OUTSTR('15&'12);
      END;

   M13X33(V[1],PTR[1,1],C[1]);

   OUTSTR('15&'12);

   OUTSTR("[");
   FOR J←1,2,3 DO OUTSTR(CVF(C[J]));
   OUTSTR("]"&'15&'12&'12);

   T33(PTR[1,1],PTR[1,1]);

   FOR I←1,2,3 DO
      BEGIN
      OUTSTR("|");
      FOR J←1,2,3 DO OUTSTR(CVF(PTR[I,J]));
      OUTSTR("|"&'15&'12);
      END;

   M13X33(C[1],PTR[1,1],C[1]);

   OUTSTR('15&'12);

   OUTSTR("[");
   FOR J←1,2,3 DO OUTSTR(CVF(C[J]));
   OUTSTR("]"&'15&'12&'12);

   T33(PTR[1,1],RTP[1,1]); M33X33(PTR[1,1],RTP[1,1],PTR[1,1]);

   FOR I←1,2,3 DO
      BEGIN
      OUTSTR("|");
      FOR J←1,2,3 DO OUTSTR(CVF(PTR[I,J]));
      OUTSTR("|"&'15&'12);
      END;

   END;

END "TSTMAT";